import rclpy 
from rclpy.node import Node
import sys
from rclpy.logging import get_logger
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations
class StaticTFBroadcasterPy(Node):
    def __init__(self):
        super().__init__("static_tf_broadcaster_py_node")
        self.broadcaster=StaticTransformBroadcaster(self)
        
    
    def pub_static_tf(self):
        ts=TransformStamped()
        ts.header.stamp=self.get_clock().now().to_msg()
        ts.header.frame_id=sys.argv[7]
        ts.child_frame_id=sys.argv[8]
        
        ts.transform._translation.x=float(sys.argv[1])
        ts.transform._translation.y=float(sys.argv[2])
        ts.transform._translation.z=float(sys.argv[3])
        
        qtn=tf_transformations.quaternion_from_euler(
            float(sys.argv[4]),
            float(sys.argv[5]),
            float(sys.argv[6])
        )
        ts.transform.rotation.x=qtn[0]
        ts.transform.rotation.y=qtn[1]
        ts.transform.rotation.z=qtn[2]
        ts.transform.rotation.w=qtn[3]

        self.broadcaster.sendTransform(ts)   

def main():
    if len(sys.argv)!=9:
        get_logger("rlcpy").error("请输入8个参数")
        return
    rclpy.init()
    publisher=StaticTFBroadcasterPy()
    publisher.pub_static_tf()
    rclpy.spin(publisher)
    rclpy.shutdown()
if __name__=='__main__':
    main()